From 3911278e2aaa01a0bd8af3555d8b811089396f5a Mon Sep 17 00:00:00 2001 From: tsteven4 <13596209+tsteven4@users.noreply.github.com> Date: Thu, 8 Aug 2024 10:07:18 -0600 Subject: [PATCH] Use Helmert transform for conversions between OSGB36 and WSG84 (#1311) * use Helmert transform for OSBG36 <-> WGS84. * adjust unicsv_grids reference for Helmert xform. * update copyright. * use constexpr to select helmert inversion method. * tweak helmert. --- garmin_txt.cc | 4 +- jeeps/gpsmath.cc | 237 ++++++++++++++++++++++++++++++++++++- jeeps/gpsmath.h | 14 +++ parse.cc | 2 +- reference/bngtest.csv | 3 + reference/bngtest.gpx | 17 +++ reference/grid-bng~csv.gpx | 28 ++--- testo.d/unicsv.test | 5 + unicsv.cc | 8 +- xcsv.cc | 4 +- 10 files changed, 298 insertions(+), 24 deletions(-) create mode 100644 reference/bngtest.csv create mode 100644 reference/bngtest.gpx diff --git a/garmin_txt.cc b/garmin_txt.cc index bf429eae0..07fd83051 100644 --- a/garmin_txt.cc +++ b/garmin_txt.cc @@ -56,7 +56,7 @@ #include "formspec.h" // for FormatSpecificDataList #include "garmin_fs.h" // for garmin_fs_t #include "garmin_tables.h" // for gt_display_modes_e, gt_find_desc_from_icon_number, gt_find_icon_number_from_desc, gt_get_mps_grid_longname, gt_lookup_datum_index, gt_lookup_grid_type, GDB, gt_get_icao_cc, gt_get_icao_country, gt_get_mps_datum_name, gt_waypt_class_names, GT_DISPLAY_MODE... -#include "jeeps/gpsmath.h" // for GPS_Math_Known_Datum_To_UTM_EN, GPS_Math_WGS84_To_Known_Datum_M, GPS_Math_WGS84_To_Swiss_EN, GPS_Math_WGS84_To_UKOSMap_M +#include "jeeps/gpsmath.h" // for GPS_Math_Known_Datum_To_UTM_EN, GPS_Math_WGS84_To_Known_Datum_M, GPS_Math_WGS84_To_Swiss_EN, GPS_Math_WGS84_To_UKOSMap_H #include "src/core/datetime.h" // for DateTime #include "src/core/logging.h" // for FatalMsg #include "src/core/textstream.h" // for TextStream @@ -243,7 +243,7 @@ GarminTxtFormat::print_position(const Waypoint* wpt) case grid_bng: - valid = GPS_Math_WGS84_To_UKOSMap_M(wpt->latitude, wpt->longitude, &east, &north, map); + valid = GPS_Math_WGS84_To_UKOSMap_H(wpt->latitude, wpt->longitude, &east, &north, map); if (valid) { *fout << QString::asprintf("%s %5.0f %5.0f\t", map, east, north); } diff --git a/jeeps/gpsmath.cc b/jeeps/gpsmath.cc index 6edfb5a14..6c539f94f 100644 --- a/jeeps/gpsmath.cc +++ b/jeeps/gpsmath.cc @@ -4,6 +4,7 @@ ** @author Copyright (C) 1999 Alan Bleasby ** @version 1.0 ** @modified Dec 28 1999 Alan Bleasby. First version +** @modified Copyright (C) 2024 Robert Lipe ** @@ ** ** This library is free software; you can redistribute it and/or @@ -23,7 +24,6 @@ ********************************************************************/ #include "jeeps/gpsmath.h" -#include // for assert #include // for sin, tan, cos, pow, log, sqrt, asin, atan, exp, fabs, round #include // for int32_t #include // for abs @@ -35,6 +35,7 @@ #include "defs.h" // for case_ignore_strcmp, fatal, CSTR #include "jeeps/gpsdatum.h" // for GPS_ODatum, GPS_OEllipse, GPS_Datums, GPS_Ellipses, UKNG, GPS_SDatum_Alias, GPS_SDatum, GPS_DatumAliases, GPS_PDatum, GPS_PDatum_Alias +static constexpr bool use_exact_helmert_inverse = false; static int32_t GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32_t* zone, char* zc, double* Mc, double* E0, @@ -1516,6 +1517,96 @@ void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa, } +/* @func GPS_Math_Helmert ******************************************* +** +** Transform one datum to another +** +** @param [r] Sx [double] source X +** @param [r] Sy [double] source Y +** @param [r] Sz [double] source Z +** @param [w] Dx [double*] converted X +** @param [w] Dy [double*] converted Y +** @param [w] Dz [double*] converted Z +** @param [r] tX [double] translation along x axis (meters) +** @param [r] tY [double] translation along y axis (meters) +** @param [r] tZ [double] translation along z axis (meters) +** @param [r] sppm [double] scale factor - 1 (ppm) +** @param [r] rXs [double] rotation about x axis (seconds) +** @param [r] rYs [double] rotation about y axis (seconds) +** @param [r] rZs [double] rotation about z axis (seconds) +** +** @return [void] +************************************************************************/ +void GPS_Math_Helmert(double Sx, double Sy, double Sz, + double* Dx, double* Dy, double* Dz, + double tX, double tY, double tZ, + double sppm, + double rXs, double rYs, double rZs) +{ + double s = sppm * 1e-6; + constexpr double radians_per_second = (1.0/(60.0*60.0)) * GPS_PI/180.0; + double rX = rXs * radians_per_second; /* radians */ + double rY = rYs * radians_per_second; /* radians */ + double rZ = rZs * radians_per_second; /* radians */ + + // rotation/scaling matrix = [1+s -rz ry;rz 1+s -rx;-ry rx 1+s]; + *Dx = tX + (1 + s)*Sx + -rZ*Sy + rY*Sz; + *Dy = tY + rZ*Sx + (1 + s)*Sy + -rX*Sz; + *Dz = tZ + -rY*Sx + rX*Sy + (1 + s)*Sz; +} + +/* @func GPS_Math_Inverse_Helmert *********************************** +** +** Transform from source to coverted datum. +** translation, scale and rotation parameters are the Helmert parameters +** to traslate from the converted to source datum! +** +** @param [r] Sx [double] source X +** @param [r] Sy [double] source Y +** @param [r] Sz [double] source Z +** @param [w] Dx [double*] converted X +** @param [w] Dy [double*] converted Y +** @param [w] Dz [double*] converted Z +** @param [r] tX [double] translation along x axis (meters) +** @param [r] tY [double] translation along y axis (meters) +** @param [r] tZ [double] translation along z axis (meters) +** @param [r] sppm [double] scale factor - 1 (ppm) +** @param [r] rXs [double] rotation about x axis (seconds) +** @param [r] rYs [double] rotation about y axis (seconds) +** @param [r] rZs [double] rotation about z axis (seconds) +** +** @return [void] +************************************************************************/ +void GPS_Math_Inverse_Helmert(double Sx, double Sy, double Sz, + double* Dx, double* Dy, double* Dz, + double tX, double tY, double tZ, + double sppm, + double rXs, double rYs, double rZs) +{ + double s = sppm * 1e-6; + constexpr double radians_per_second = (1.0/(60.0*60.0)) * GPS_PI/180.0; + double rX = rXs * radians_per_second; /* radians */ + double rY = rYs * radians_per_second; /* radians */ + double rZ = rZs * radians_per_second; /* radians */ + + // forward rotation/scaling matrix is [1+s -rz ry;rz 1+s -rx;-ry rx 1+s] + // compute inverse of forward Helmert rotation/scaling matrix + // https://www.wolframalpha.com/input?i2d=true&i=Power%5B%7B%7B1%2Bs%2C-Subscript%5Br%2Cz%5D%2CSubscript%5Br%2Cy%5D%7D%2C%7BSubscript%5Br%2Cz%5D%2C1%2Bs%2C-Subscript%5Br%2Cx%5D%7D%2C%7B-Subscript%5Br%2Cy%5D%2CSubscript%5Br%2Cx%5D%2C1%2Bs%7D%7D%2C-1%5D + double gain = 1/((s + 1) * (rX*rX + rY*rY + rZ*rZ + s*s + 2*s + 1)); + double r11 = rX*rX + s*s + 2*s + 1; + double r12 = s*rZ + rX*rY + rZ; + double r13 = -s*rY + rX*rZ - rY; + double r21 = -s*rZ + rX*rY - rZ; + double r22 = rY*rY + s*s + 2*s + 1; + double r23 = s*rX + rX + rY*rZ; + double r31 = s*rY + rX*rZ + rY; + double r32 = -s*rX - rX + rY*rZ; + double r33 = rZ*rZ + s*s + 2*s + 1; + + *Dx = gain * ((r11 * (Sx-tX)) + (r12 * (Sy-tY)) + (r13 * (Sz-tZ))); + *Dy = gain * ((r21 * (Sx-tX)) + (r22 * (Sy-tY)) + (r23 * (Sz-tZ))); + *Dz = gain * ((r31 * (Sx-tX)) + (r32 * (Sy-tY)) + (r33 * (Sz-tZ))); +} /* @func GPS_Math_Known_Datum_To_WGS84_M ********************************** ** @@ -1904,6 +1995,150 @@ int32_t GPS_Math_UKOSMap_To_WGS84_M(const char* map, double mE, double mN, } +/* @func GPS_Math_WGS84_To_UKOSMap_H *********************************** +** +** Convert WGS84 lat/lon to Ordnance survey map code and easting and +** northing. Uses Helmert. +** +** @param [r] lat [double] WGS84 latitude (deg) +** @param [r] lon [double] WGS84 longitude (deg) +** @param [w] mE [double *] map easting (metres) +** @param [w] mN [double *] map northing (metres) +** @param [w] map [char *] map two letter code +** +** @return [int32] success +************************************************************************/ +int32_t GPS_Math_WGS84_To_UKOSMap_H(double lat, double lon, double* mE, + double* mN, char* map) +{ + double x; + double y; + double z; + double ax; + double ay; + double az; + double alat; + double alon; + double ah; + double aE; + double aN; + + + GPS_Math_WGS84LatLonH_To_XYZ(lat, lon, 0, + &x, &y, &z); + + /* Helmert transformation + * https://www.ordnancesurvey.co.uk/documents/resources/guide-coordinate-systems-great-britain.pdf + * 6.2 Helmert datum transformations + * 6.6 Approximate WGS84 to OSGB36/ODN transformation + */ + // WGS84 -> OSGB36 + constexpr double tX = -446.448; /* meters */ + constexpr double tY = +125.157; /* meters */ + constexpr double tZ = -542.060; /* meters */ + constexpr double s = +20.4894; /* ppm */ + constexpr double rX = -0.1502; /* seconds */ + constexpr double rY = -0.2470; /* seconds */ + constexpr double rZ = -0.8421; /* seconds */ + + GPS_Math_Helmert(x, y, z, + &ax, &ay, &az, + tX, tY, tZ, + s, + rX, rY, rZ); + + GPS_Math_XYZ_To_Airy1830LatLonH(&alat, &alon, &ah, + ax, ay, az); + GPS_Math_Airy1830LatLonToNGEN(alat, alon, + &aE, &aN); + if (!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map)) { + return 0; + } + + return 1; +} + + +/* @func GPS_Math_UKOSMap_To_WGS84_H *********************************** +** +** Transform UK Ordnance survey map position to WGS84 lat/lon +** Uses Helmert transformation +** +** @param [r] map [const char *] map two letter code +** @param [r] mE [double] map easting (metres) +** @param [r] mN [double] map northing (metres) +** @param [w] lat [double *] WGS84 latitude (deg) +** @param [w] lon [double *] WGS84 longitude (deg) +** +** @return [int32] success +************************************************************************/ +int32_t GPS_Math_UKOSMap_To_WGS84_H(const char* map, double mE, double mN, + double* lat, double* lon) +{ + double E; + double N; + double alat; + double alon; + double ht; + double ax; + double ay; + double az; + double x; + double y; + double z; + + if (!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N)) { + return 0; + } + + GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon); + GPS_Math_Airy1830LatLonH_To_XYZ(alat, alon, 0.0, &ax, &ay, &az); + + /* Helmert transformation + * https://www.ordnancesurvey.co.uk/documents/resources/guide-coordinate-systems-great-britain.pdf + * 6.2 Helmert datum transformations + * 6.6 Approximate WGS84 to OSGB36/ODN transformation + */ + if constexpr(use_exact_helmert_inverse) { + // Actually invert the Helmert transform from WGS84 to OSGB36. + // WGS84 -> OSGB36 + constexpr double tX = -446.448; /* meters */ + constexpr double tY = +125.157; /* meters */ + constexpr double tZ = -542.060; /* meters */ + constexpr double s = +20.4894; /* ppm */ + constexpr double rX = -0.1502; /* seconds */ + constexpr double rY = -0.2470; /* seconds */ + constexpr double rZ = -0.8421; /* seconds */ + + GPS_Math_Inverse_Helmert(ax, ay, az, + &x, &y, &z, + tX, tY, tZ, + s, + rX, rY, rZ); + } else { + // Approximate the transform from OSGB36 to WGS84 by using the standard + // helmert transform with parameters that all have the opposite signs of + // those used to transform from WGS84 to OSGB36. + constexpr double tX = +446.448; /* meters */ + constexpr double tY = -125.157; /* meters */ + constexpr double tZ = +542.060; /* meters */ + constexpr double s = -20.4894; /* ppm */ + constexpr double rX = +0.1502; /* seconds */ + constexpr double rY = +0.2470; /* seconds */ + constexpr double rZ = +0.8421; /* seconds */ + + GPS_Math_Helmert(ax, ay, az, + &x, &y, &z, + tX, tY, tZ, + s, + rX, rY, rZ); + } + + GPS_Math_XYZ_To_WGS84LatLonH(lat, lon, &ht, x, y, z); + + return 1; +} + /* @func GPS_Math_WGS84_To_UKOSMap_C *********************************** ** diff --git a/jeeps/gpsmath.h b/jeeps/gpsmath.h index 19451987d..feac1c97b 100644 --- a/jeeps/gpsmath.h +++ b/jeeps/gpsmath.h @@ -72,6 +72,16 @@ void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa, double Sif, double* Dphi, double* Dlam, double* DH, double Da, double Dif, double dx, double dy, double dz); +void GPS_Math_Helmert(double Sx, double Sy, double Sz, + double* Dx, double* Dy, double* Dz, + double tX, double tY, double tZ, + double sppm, + double rXs, double rYs, double rZs); +void GPS_Math_Inverse_Helmert(double Sx, double Sy, double Sz, + double* Dx, double* Dy, double* Dz, + double tX, double tY, double tZ, + double sppm, + double rXs, double rYs, double rZs); void GPS_Math_Known_Datum_To_WGS84_M(double Sphi, double Slam, double SH, double* Dphi, double* Dlam, double* DH, int32_t n); @@ -96,6 +106,10 @@ int32_t GPS_Math_WGS84_To_UKOSMap_M(double lat, double lon, double* mE, double* mN, char* map); int32_t GPS_Math_UKOSMap_To_WGS84_M(const char* map, double mE, double mN, double* lat, double* lon); +int32_t GPS_Math_WGS84_To_UKOSMap_H(double lat, double lon, double* mE, + double* mN, char* map); +int32_t GPS_Math_UKOSMap_To_WGS84_H(const char* map, double mE, double mN, + double* lat, double* lon); int32_t GPS_Math_WGS84_To_UKOSMap_C(double lat, double lon, double* mE, double* mN, char* map); int32_t GPS_Math_UKOSMap_To_WGS84_C(const char* map, double mE, double mN, diff --git a/parse.cc b/parse.cc index bd03cb42e..5dacd5016 100644 --- a/parse.cc +++ b/parse.cc @@ -221,7 +221,7 @@ parse_coordinates(const char* str, int datum, const grid_type grid, &result); valid = (ct == 3); if (valid) { - if (! GPS_Math_UKOSMap_To_WGS84_M(map, lx, ly, &lat, &lon)) + if (! GPS_Math_UKOSMap_To_WGS84_H(map, lx, ly, &lat, &lon)) fatal("%s: Unable to convert BNG coordinates (%s)!\n", module, str); } diff --git a/reference/bngtest.csv b/reference/bngtest.csv new file mode 100644 index 000000000..c3cf5be0d --- /dev/null +++ b/reference/bngtest.csv @@ -0,0 +1,3 @@ +No,BNG-Zone,BNG-East,BNG-North,Name,Description,Date,Time +1,ST,96552, 3097,"Badbury Rings Trig Point","Test Point 1",2024/08/05,00:00:00 +2,ST,91437, 1894,"Spettisbury Rings Trig Point","Test Point 2",2024/08/05,00:00:00 diff --git a/reference/bngtest.gpx b/reference/bngtest.gpx new file mode 100644 index 000000000..cc75b4677 --- /dev/null +++ b/reference/bngtest.gpx @@ -0,0 +1,17 @@ + + + + + + + Badbury Rings Trig Point + Test Point 1 + Test Point 1 + + + + Spettisbury Rings Trig Point + Test Point 2 + Test Point 2 + + diff --git a/reference/grid-bng~csv.gpx b/reference/grid-bng~csv.gpx index ddcb600ea..babec11c2 100644 --- a/reference/grid-bng~csv.gpx +++ b/reference/grid-bng~csv.gpx @@ -1,92 +1,92 @@ - - + + AlineLodge Aline Lodge Aline Lodge City (Small) - + Caernarfon Caernarfon Caernarfon City (Small) - + Camborne Camborne Camborne City (Small) - + Exeter Exeter Exeter City (Medium) - + Forfar Forfar Forfar City (Small) - + Hawick Hawick Hawick City (Small) - + Hosta Hosta Hosta City (Small) - + IsleofRhum Isle of Rhum Isle of Rhum City (Small) - + Lerwick Lerwick Lerwick City (Small) - + Nethertown Nethertown Nethertown City (Small) - + Norwich Norwich Norwich City (Medium) - + Selsey Selsey Selsey City (Small) - + Sheffield Sheffield diff --git a/testo.d/unicsv.test b/testo.d/unicsv.test index 99bfdc681..93a89e341 100644 --- a/testo.d/unicsv.test +++ b/testo.d/unicsv.test @@ -59,3 +59,8 @@ compare ${REFERENCE}/pretty_degree2.csv ${TMPDIR}/pretty_degree2.csv gpsbabel -i unicsv -f ${REFERENCE}/unidelim.csv -o gpx -F ${TMPDIR}/unidelim.gpx compare ${REFERENCE}/unidelim.gpx ${TMPDIR}/unidelim.gpx +gpsbabel -i unicsv,utc -f ${REFERENCE}/bngtest.csv -o gpx -F ${TMPDIR}/bngtest~csv.gpx +compare ${REFERENCE}/bngtest.gpx ${TMPDIR}/bngtest~csv.gpx +gpsbabel -i gpx -f ${REFERENCE}/bngtest.gpx -o unicsv,grid=bng,utc -F ${TMPDIR}/bngtest~gpx.csv +compare ${REFERENCE}/bngtest.csv ${TMPDIR}/bngtest~gpx.csv + diff --git a/unicsv.cc b/unicsv.cc index ab1534de9..84800e98e 100644 --- a/unicsv.cc +++ b/unicsv.cc @@ -46,7 +46,7 @@ #include "garmin_fs.h" // for garmin_fs_t #include "garmin_tables.h" // for gt_lookup_datum_index, gt_get_mps_grid_longname, gt_lookup_grid_type #include "geocache.h" // for Geocache, Geocache::status_t, Geoc... -#include "jeeps/gpsmath.h" // for GPS_Math_UKOSMap_To_WGS84_M, GPS_Math_EN_To_UKOSNG_Map, GPS_Math_Known_Datum_To_UTM_EN, GPS_Math_Known_Datum_To_WGS84_M, GPS_Math_Swiss_EN_To_WGS84, GPS_Math_UTM_EN_To_Known_Datum, GPS_Math_WGS84_To_Known_Datum_M, GPS_Math_WGS84_To_Swiss_EN, GPS_Math_WGS... +#include "jeeps/gpsmath.h" // for GPS_Math_UKOSMap_To_WGS84_H, GPS_Math_EN_To_UKOSNG_Map, GPS_Math_Known_Datum_To_UTM_EN, GPS_Math_Known_Datum_To_WGS84_M, GPS_Math_Swiss_EN_To_WGS84, GPS_Math_UTM_EN_To_Known_Datum, GPS_Math_WGS84_To_Known_Datum_M, GPS_Math_WGS84_To_Swiss_EN, GPS_Math_WGS... #include "session.h" // for session_t #include "src/core/datetime.h" // for DateTime #include "src/core/logging.h" // for Warning, Fatal @@ -1017,13 +1017,13 @@ UnicsvFormat::unicsv_parse_one_line(const QString& ibuf) fatal(MYNAME ": Unable to convert BNG coordinates (%.f %.f)!\n", bng_easting, bng_northing); } - if (! GPS_Math_UKOSMap_To_WGS84_M( + if (! GPS_Math_UKOSMap_To_WGS84_H( bngz, bnge, bngn, &wpt->latitude, &wpt->longitude)) fatal(MYNAME ": Unable to convert BNG coordinates (%s %.f %.f)!\n", bngz, bnge, bngn); } else { // traditional zone easting northing - if (! GPS_Math_UKOSMap_To_WGS84_M( + if (! GPS_Math_UKOSMap_To_WGS84_H( CSTR(bng_zone), bng_easting, bng_northing, &wpt->latitude, &wpt->longitude)) fatal(MYNAME ": Unable to convert BNG coordinates (%s %.f %.f)!\n", @@ -1322,7 +1322,7 @@ UnicsvFormat::unicsv_waypt_disp_cb(const Waypoint* wpt) double north; double east; - if (! GPS_Math_WGS84_To_UKOSMap_M(wpt->latitude, wpt->longitude, &east, &north, map)) { + if (! GPS_Math_WGS84_To_UKOSMap_H(wpt->latitude, wpt->longitude, &east, &north, map)) { unicsv_fatal_outside(wpt); } auto fieldWidth = fout->fieldWidth(); diff --git a/xcsv.cc b/xcsv.cc index 23266cd6a..c64d9c4d8 100644 --- a/xcsv.cc +++ b/xcsv.cc @@ -56,7 +56,7 @@ #include "garmin_fs.h" // for garmin_fs_t #include "geocache.h" // for Geocache, Geocache::status_t, Geoc... #include "grtcirc.h" // for RAD, gcdist, radtometers -#include "jeeps/gpsmath.h" // for GPS_Math_WGS84_To_UTM_EN, GPS_Lookup_Datum_Index, GPS_Math_Known_Datum_To_WGS84_M, GPS_Math_UTM_EN_To_Known_Datum, GPS_Math_WGS84_To_Known_Datum_M, GPS_Math_WGS84_To_UKOSMap_M +#include "jeeps/gpsmath.h" // for GPS_Math_WGS84_To_UTM_EN, GPS_Lookup_Datum_Index, GPS_Math_Known_Datum_To_WGS84_M, GPS_Math_UTM_EN_To_Known_Datum, GPS_Math_WGS84_To_Known_Datum_M, GPS_Math_WGS84_To_UKOSMap_H #include "jeeps/gpsport.h" // for int32 #include "session.h" // for session_t #include "src/core/datetime.h" // for DateTime @@ -1227,7 +1227,7 @@ XcsvFormat::xcsv_waypt_pr(const Waypoint* wpt) char map[3]; double north; double east; - if (! GPS_Math_WGS84_To_UKOSMap_M(wpt->latitude, wpt->longitude, &east, &north, map)) + if (! GPS_Math_WGS84_To_UKOSMap_H(wpt->latitude, wpt->longitude, &east, &north, map)) fatal(MYNAME ": Position (%.5f/%.5f) outside of BNG.\n", wpt->latitude, wpt->longitude); buff = QString::asprintf(fmp.printfc.constData(), map, qRound(east), qRound(north)); -- 2.30.2